#include <windows.h>
#include <tchar.h>
#include <stdio.h>

#include "ImgRegistration.h"
#include "ExrFileIO.h"

#define DISPSCALE 0.35f
#define FEATUREFILE "FeaturePts.txt"
#define TESTCIRCLEX 900
#define TESTCIRCLEY	406

void MapCoordsBackToOrgImg(CImgRegistration* pImgInfo, float x, float y, float& x_org, float& y_org)
{
	x_org = (( x / pImgInfo->GetScaledImgPointer()->GetImgPointer()->width) * pImgInfo->GetImgPointer()->GetImgPointer()->width);
	y_org = (( y / pImgInfo->GetScaledImgPointer()->GetImgPointer()->height) * pImgInfo->GetImgPointer()->GetImgPointer()->height);
}

void findCornersInSquare(CImgRegistration* pImgInfo, float x, float y, bool bScaleMapping = true)
{
	float x_org = x;
	float y_org = y;
	//map the x,y coords back to the orignial image if it has been scaled
	if(pImgInfo->GetScaledImgPointer()->GetImgPointer()!=NULL && bScaleMapping)
		MapCoordsBackToOrgImg(pImgInfo,x,y,x_org,y_org);

	int iNeighborSize = pImgInfo->GetNumNeighborSize();
	int iSquareSize = 2*iNeighborSize+1;
	Rect r((int)(x_org-iNeighborSize), (int)(y_org-iNeighborSize),iSquareSize,iSquareSize);


	printf("rect:(%d,%d,%d,%d)\n",r.x,r.y,r.width,r.height);
	IplImage* LocalROI = cvCreateImage(cvSize(iSquareSize,iSquareSize),IPL_DEPTH_8U,1);
	//copy the content from iplimage to mat
	pImgInfo->GetImgPointer()->CopyImageRegionToSingleChannelMat(LocalROI,r);
	//DEBUG
	cvSaveImage("LocalROI.jpg",LocalROI);
	//detect corner
	int corner_count = 1;
	IplImage* eig_img = cvCreateImage (cvGetSize (LocalROI), IPL_DEPTH_32F, 1);
	IplImage* temp_img = cvCreateImage (cvGetSize (LocalROI), IPL_DEPTH_32F, 1);
	CvPoint2D32f corner;

	// (1)Corner detection using cvCornerHarris
	cvGoodFeaturesToTrack (LocalROI, eig_img, temp_img, &corner, &corner_count, 0.1, 15, NULL, 3, 1, 0.04);
	// (2)cvGoodFeaturesToTrack (src_img_gray, eig_img, temp_img, corners, &corner_count, 0.1, 15);
	cvFindCornerSubPix (LocalROI, &corner, corner_count,
		cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
	// (3)shift the corner point
	CvPoint2D32f FinalCorner;
	FinalCorner.x = corner.x + r.x;
	FinalCorner.y = corner.y + r.y;
	pImgInfo->GetArrCorners().push_back(FinalCorner);
	printf("%f,%f\n",FinalCorner.x,FinalCorner.y);

	// (4)Draw the detected corner
#if 1
	cvCircle (LocalROI, cvPointFrom32f (corner), 7, CV_RGB (0, 0, 255));
	cvSaveImage("LocalROI.jpg",LocalROI);
#endif
	cvReleaseImage(&LocalROI);
}

void ShowTheCornerPoints(CImgRegistration* pImgInfo)
{
	for(int i = 0; i < pImgInfo->GetArrCorners().size(); i++)
	{
		CvPoint2D32f circleCenter = pImgInfo->GetArrCorners()[i];
		if (pImgInfo->GetScaledImgPointer()->GetImgPointer()!=NULL)
		{
			circleCenter.x = (circleCenter.x/pImgInfo->GetImgPointer()->GetImgPointer()->width) * pImgInfo->GetScaledImgPointer()->GetImgPointer()->width;
			circleCenter.y = (circleCenter.y/pImgInfo->GetImgPointer()->GetImgPointer()->height) * pImgInfo->GetScaledImgPointer()->GetImgPointer()->height;
		}
		cvCircle (pImgInfo->GetScaledImgPointer()->GetImgPointer(), cvPointFrom32f (circleCenter), 15, CV_RGB (0, 0, 255),3);
	}
}

void mouseHandler(int event, int x, int y, int flags, void *param)
{
	CImgRegistration* pImgInfo = (CImgRegistration*)(param);
	switch(event) {
	case CV_EVENT_LBUTTONDOWN:{		//left button press
		if(pImgInfo->GetArrCorners().size() == pImgInfo->GetNumFeaturePts())
			printf("Reach maximum number of feature pts!\n");
		else{
			float x1 = x;
			float y1 = y;

			if(x1>=0 && y1>=0)
				findCornersInSquare(pImgInfo, x1, y1);
		}

							  }
							  break;

	case CV_EVENT_LBUTTONUP:{	//left mouse button release
		ShowTheCornerPoints(pImgInfo);
		cvShowImage("Drag & Drop", pImgInfo->GetScaledImgPointer()->GetImgPointer());
							}
							break;

	}
}


int main(int argc, char** argv)
{
	//img0 = cvLoadImage("test1.jpg", CV_LOAD_IMAGE_COLOR);
	CImgRegistration* pImgInfo = new CImgRegistration();
	pImgInfo->LoadImageForDisplay("hdr.0000.png", true, DISPSCALE);
	//save first hdr file
	CExrFileIO exrFile1;
	exrFile1.LoadExrFile("GantryImgs\\HDR_unalign\\hdr.0000.exr");
	exrFile1.SaveExrFile("GantryImgs\\HDR_align\\hdr.0000.exr",WRITE_Y);
	//pImgInfo->LoadImageForDisplay("CapTest2-00000-00003.jpg", true);
	//imgInfo.m_pImg = cvLoadImage("CapTest2-00000-00003_s_test.png", CV_LOAD_IMAGE_COLOR);
	//imgInfo.pImg = cvLoadImage("CapTest2-00001-00003_s_test.png", CV_LOAD_IMAGE_COLOR);
	//scaled img for displaying
	CImgRegistration* pImgInfo1 = new CImgRegistration();
	cvNamedWindow("Propagated image 1",CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED);

	//cvNamedWindow("Drag & Drop",CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED);
	cvNamedWindow("Drag & Drop",CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED);
	cvSetMouseCallback( "Drag & Drop", mouseHandler, (void*)(pImgInfo) );
	cvShowImage("Drag & Drop", pImgInfo->GetScaledImgPointer()->GetImgPointer());
	//cvWaitKey(0);
	while(true)
	{
		char key = waitKey(0);

		if (key == 'p')
		{
			if(pImgInfo->GetArrCorners().size() != pImgInfo->GetNumFeaturePts())
				printf("Pls fill enough feature pts firstly!\n");
			else
			{
				std::vector<TransFormData_2D> transformArr;
				WIN32_FIND_DATA FindFileData;
				HANDLE hFind = FindFirstFile(TEXT("GantryImgs\\*.png"), &FindFileData);
				while (hFind != INVALID_HANDLE_VALUE)
				{
					//do stuff with FindFileData Here
					UtilityString strCurDir("GantryImgs\\");
					UtilityString strFileName(FindFileData.cFileName);
					strCurDir.ConcatenateUtilityStr(strFileName);
					printf("Start Process %s.\n", strCurDir.GetCharStr());

					//printf("start propagating the local regions!\n");
					pImgInfo1->LoadImageForDisplay(strCurDir.GetCharStr(), true, DISPSCALE);
					//imgInfo.pImg = cvLoadImage("CapTest2-00001-00003_s_test.png", CV_LOAD_IMAGE_COLOR);

					//find the corners
					pImgInfo1->GetArrCorners().clear();
					for (int i = 0; i < pImgInfo->GetArrCorners().size(); i++)
					{
						findCornersInSquare(pImgInfo1,pImgInfo->GetArrCorners()[i].x,pImgInfo->GetArrCorners()[i].y,false);
					}
					//ShowTheCornerPoints(pImgInfo1);
					//cvShowImage("Propagated image 1", pImgInfo1->GetScaledImgPointer()->GetImgPointer());

					//compute affine transform between two images
					pImgInfo->ComputeAffineTransform(pImgInfo1);
					TransFormData_2D trans = pImgInfo->Get2DTransform();
					transformArr.push_back(trans);

					UtilityString strHDRUnAlignDir("GantryImgs\\HDR_unalign\\");
					UtilityString strHDRAlignDir("GantryImgs\\HDR_align\\");
					UtilityString strFileNameExr(strFileName);
					strFileNameExr.replaceExt("exr");
					strHDRUnAlignDir.ConcatenateUtilityStr(strFileNameExr);
					strHDRAlignDir.ConcatenateUtilityStr(strFileNameExr);

					CExrFileIO exrFile;
					exrFile.LoadExrFile(strHDRUnAlignDir.GetCharStr());
					exrFile.SaveWarpedExrFile(strHDRAlignDir.GetCharStr(),trans, WRITE_Y);

#if 0
					//save circled image
					//draw circle for marking!
					cvCircle (pImgInfo1->GetImgPointer()->GetImgPointer(), cvPoint(TESTCIRCLEX,TESTCIRCLEY), 15, CV_RGB (0, 0, 255),5);
					ZeroMemory(strCurDir,sizeof(char)*MAX_PATH);
					sprintf_s(strCurDir,"GantryImgs\\Circled\\");
					strcat(strCurDir,strFileName);
					cvSaveImage(strCurDir, pImgInfo1->GetImgPointer()->GetImgPointer());
					printf("%s saved!\n",strCurDir);

					//create warped image
					COpenCVImg* pWarpedImg = new COpenCVImg();
					pWarpedImg->CreateImg(cvSize(pImgInfo1->GetImgPointer()->GetWidth(), pImgInfo1->GetImgPointer()->GetHeight()), 
						pImgInfo1->GetImgPointer()->GetDepth(), pImgInfo1->GetImgPointer()->GetChannels());
					pWarpedImg->FillImageRegionWithSingleColor(Rect(0,0,pImgInfo1->GetImgPointer()->GetWidth(), pImgInfo1->GetImgPointer()->GetHeight()), 255,0,255);
					//warping
					if(!pImgInfo->isTransOnly())
					{
						printf("More than translation involved!\n");
						continue;
					}

					//start warping
					for( int y=0; y<pImgInfo1->GetImgPointer()->GetHeight(); y++ ) { 
						uchar* ptr1 = (uchar*) ( pImgInfo1->GetImgPointer()->GetImgPointer()->imageData + y * pImgInfo1->GetImgPointer()->GetImgPointer()->widthStep ); 
						for( int x=0; x<pImgInfo1->GetImgPointer()->GetWidth(); x++ ) {

							float xPos = x + pImgInfo->Get2DTransform().m_fTransX + 0.5f;
							float yPos = y + pImgInfo->Get2DTransform().m_fTransY + 0.5f;

							if(xPos < 0 || yPos < 0 || xPos >= pImgInfo1->GetImgPointer()->GetWidth() || yPos >= pImgInfo1->GetImgPointer()->GetHeight() )
								continue;

							int xIdx = (int)(floorf(xPos));
							int yIdx = (int)(floorf(yPos));

							uchar* ptr = (uchar*) ( pWarpedImg->GetImgPointer()->imageData + yIdx * pWarpedImg->GetImgPointer()->widthStep ); 

							ptr[3*xIdx] = ptr1[3*x]; 
							ptr[3*xIdx+1] = ptr1[3*x+1]; 
							ptr[3*xIdx+2] = ptr1[3*x+2];
						} //Set red to max (BGR format)
					}

					//draw circle for marking!
					cvCircle (pWarpedImg->GetImgPointer(), cvPoint(TESTCIRCLEX,TESTCIRCLEY), 15, CV_RGB (0, 0, 255),5);

					ZeroMemory(strCurDir,sizeof(char)*MAX_PATH);
					sprintf_s(strCurDir,"GantryImgs\\Warpped\\");
					strcat(strCurDir,strFileName);
					cvSaveImage(strCurDir, pWarpedImg->GetImgPointer());
					printf("%s saved!\n",strCurDir);

					delete(pWarpedImg);
#endif
					if (!FindNextFile(hFind, &FindFileData))
					{
						FindClose(hFind);
						hFind = INVALID_HANDLE_VALUE;
					}
				}

				UtilityString strTransFN("GantryImgs\\Warpped\\");
				strTransFN.ConcatenateStr("SeqTrans.txt");
				FILE* fp1 = fopen(strTransFN.GetCharStr(),"w");
				for (int iTrans = 0; iTrans < transformArr.size(); iTrans++)
				fprintf(fp1,"cosTheta=%f,sinTheta=%f,transX=%f,transY=%f\n",transformArr[iTrans].m_fCosTheta,transformArr[iTrans].m_fSinTheta,
						transformArr[iTrans].m_fTransX, transformArr[iTrans].m_fTransY);
				fclose(fp1);


			}			

		}
		else if (key == 's')//save feature pts files
		{
			if(pImgInfo->GetArrCorners().size() == pImgInfo->GetNumFeaturePts())
			{
				pImgInfo->SaveFeaturePts(FEATUREFILE);
			}
		}
		else if (key == 'l')//load pre-saved feature pts files
		{
			FILE* fp = fopen(FEATUREFILE, "r");
			if(fp != NULL)
			{
				int iNumPts;
				fscanf(fp,"%d\n",&iNumPts);
				if(iNumPts == pImgInfo->GetNumFeaturePts())
				{
					pImgInfo->GetArrCorners().clear();
					while(!feof(fp))
					{
						CvPoint2D32f pt;
						fscanf(fp, "%f %f\n",&(pt.x),&(pt.y) );
						pImgInfo->GetArrCorners().push_back(pt);
					}
				}
				ShowTheCornerPoints(pImgInfo);
				cvShowImage("Drag & Drop", pImgInfo->GetScaledImgPointer()->GetImgPointer());
			
				fclose(fp);
			}
	    }		
		else if (key == 27)//ascii code of 'esc'
			break;
	}

	//cvDestroyWindow("Propagated image 1");
	cvSaveImage("Corners.jpg",pImgInfo->GetImgPointer()->GetImgPointer());

	cvDestroyWindow("Propagated image 1");
	cvDestroyWindow("Drag & Drop");
	//delete resource
	delete(pImgInfo1);	
	delete(pImgInfo);
	return 0;
}




